/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package com.team1160.breakaway.pid;

import com.team1160.breakaway.api.Constants;

/**
 *
 * @author Saketh Kasibatla
 */
public class PID {
    double totalError=0,lastError=0;
    double lastSetValue;
    //TODO finetune delta
    double delta = .05;

    public PID(){}

    public double calculate(double setValue,double currentValue){
        if(!withinDelta(lastSetValue,setValue)){
            totalError=0;
            lastError=0;
        }
        lastSetValue=setValue;
        double error = setValue - currentValue;

        totalError+=error;
        double P = Constants.KP*error;
        double I = Constants.KI*totalError;
        double D = Constants.KD*(error-lastError);
        lastError=error;
        return P+I+D;
    }

    private boolean withinDelta(double last,double current){
        if((current>=(last-delta))&&(current<=(last+delta)))
            return true;
        return false;
    }
}

